Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation

This paper derives a contact-aided inertial navigation observer for a 3D bipedal robot using the theory of invariant observer design. Aided inertial navigation is fundamentally a nonlinear observer design problem; thus, current solutions are based on approximations of the system dynamics, such as an Extended Kalman Filter (EKF), which uses a system's Jacobian linearization along the current best estimate of its trajectory. On the basis of the theory of invariant observer design by Barrau and Bonnabel, and in particular, the Invariant EKF (InEKF), we show that the error dynamics of the point contact-inertial system follows a log-linear autonomous differential equation; hence, the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory. Due to the log-linear form of the error dynamics, it is not necessary to perform a nonlinear observability analysis to show that when using an Inertial Measurement Unit (IMU) and contact sensors, the absolute position of the robot and a rotation about the gravity vector (yaw) are unobservable. We further augment the state of the developed InEKF with IMU biases, as the online estimation of these parameters has a crucial impact on system performance. We evaluate the convergence of the proposed system with the commonly used quaternion-based EKF observer using a Monte-Carlo simulation. In addition, our experimental evaluation using a Cassie-series bipedal robot shows that the contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in the system dynamics.Comment: Published in the proceedings of Robotics: Science and Systems 201

Gaussian processes for information-theoretic robotic mapping and exploration

University of Technology Sydney. Faculty of Engineering and Information Technology.This thesis proposes a framework for autonomous robotic mapping, exploration, and planning that uses Gaussian Processes (GPs) to model high-dimensional dense maps and solve the problem of infinite-horizon planning with imperfect state information. Robotic exploration is traditionally implemented using occupancy grid representations and geometric targets known as frontiers. The occupancy grid representation relies on the assumption of independence between grid cells and ignores structural correlations present in the environment. We develop an incremental GP occupancy mapping technique that is computationally tractable for online map building and represents a continuous model of uncertainty over the map spatial coordinates. The standard way to represent geometric frontiers extracted from occupancy maps is to assign binary values to each grid cell. We extend this notion to novel probabilistic frontier maps computed efficiently using the gradient of the GP occupancy map and propose a mutual information-based greedy exploration technique built on that representation. A primary motivation is the fact that high-dimensional map inference requires fewer observations, leading to a faster map entropy reduction during exploration for map building scenarios. The uncertainty from pose estimation is often ignored during current mapping strategies as the dense belief representation of occupancy maps makes the uncertainty propagation impractical. Additionally, when kernel methods are applied, such maps tend to model structural shapes of the environment with excessive smoothness. We show how the incremental GP occupancy mapping technique can be extended to accept uncertain robot poses and mitigate the excessive smoothness problem using Warped Gaussian Processes. This approach can model non-Gaussian noise in the observation space and capture the possible non-linearity in that space better than standard GPs. Finally, we develop a sampling-based information gathering planner, with an information-theoretic convergence, which allows dense belief representations. The planner takes the present uncertainty in state estimation into account and provides a general framework for robotic exploration in a priori unknown environments with an information-theoretic stopping criterion. The developed framework relaxes the need for any state or action space discretization and is a fully information-driven integrated navigation technique. The developed framework can be applied to a large number of scenarios where the robot is tasked to perform exploration and information gathering simultaneously. The developed algorithms in this thesis are implemented and evaluated using simulated and experimental datasets and are publicly available as open source libraries

Bayesian dense inverse searching algorithm for real-time stereo matching in minimally invasive surgery

This paper reports a CPU-level real-time stereo matching method for surgical images (10 Hz on 640 * 480 image with a single core of i5-9400). The proposed method is built on the fast ''dense inverse searching'' algorithm, which estimates the disparity of the stereo images. The overlapping image patches (arbitrary squared image segment) from the images at different scales are aligned based on the photometric consistency presumption. We propose a Bayesian framework to evaluate the probability of the optimized patch disparity at different scales. Moreover, we introduce a spatial Gaussian mixed probability distribution to address the pixel-wise probability within the patch. In-vivo and synthetic experiments show that our method can handle ambiguities resulted from the textureless surfaces and the photometric inconsistency caused by the Lambertian reflectance. Our Bayesian method correctly balances the probability of the patch for stereo images at different scales. Experiments indicate that the estimated depth has higher accuracy and fewer outliers than the baseline methods in the surgical scenario

Convex Geometric Motion Planning on Lie Groups via Moment Relaxation

This paper reports a novel result: with proper robot models on matrix Lie groups, one can formulate the kinodynamic motion planning problem for rigid body systems as \emph{exact} polynomial optimization problems that can be relaxed as semidefinite programming (SDP). Due to the nonlinear rigid body dynamics, the motion planning problem for rigid body systems is nonconvex. Existing global optimization-based methods do not properly deal with the configuration space of the 3D rigid body; thus, they do not scale well to long-horizon planning problems. We use Lie groups as the configuration space in our formulation and apply the variational integrator to formulate the forced rigid body systems as quadratic polynomials. Then we leverage Lasserre's hierarchy to obtain the globally optimal solution via SDP. By constructing the motion planning problem in a sparse manner, the results show that the proposed algorithm has \emph{linear} complexity with respect to the planning horizon. This paper demonstrates the proposed method can provide rank-one optimal solutions at relaxation order two for most of the testing cases of 1) 3D drone landing using the full dynamics model and 2) inverse kinematics for serial manipulators.Comment: Accepted to Robotics: Science and Systems (RSS), 202